#define _USE_MATH_DEFINES

#include <iostream>
#include <fstream>
#include <datatable.h>
#include <bspline.h>
#include <bsplinebuilder.h>
#include <nlohmann/json.hpp>
#include <vector>
#include <cmath>

#include "helper.h"

#include "sampler2.h"
#include "ppformbuilder.h"
#include <math.h>

using namespace SPLINTER;

double chordInterpolation(DataTable &samples_table, const ISampler &sampler, double start, double end, int N = 10240);
size_t getTestTheta(std::vector<DenseVector> &testThetas, const Function &function, double chord_sum, size_t N = 4096);
typedef void (*GetXY)(DenseVector theta, double &x, double &y, const std::map<std::string, void *> &args);
void plotFunctionByTheta(const std::vector<DenseVector> &testThetas, GetXY getxy, const std::map<std::string, std::string> &keywords = {}, const std::map<std::string, void *> &args = {});
DenseVector getTheta(const Function &function, double chord);

int main(int argc, char *argv[])
{
    Sampler2 sampler;
    double start = -M_PI / 2, end = M_PI * 3 / 2;
    int Ns[] = {10, 40, 160, 512, 1024, 2048};
    int degrees[] = {0, 1, 2, 3, 4, 5};
    std::vector<BSpline> splines;
    std::vector<DenseVector> testThetas;
    DataTable samples_table;
    double chord_sum = chordInterpolation(samples_table, sampler, start, end);
    BSpline bspline_sampler = BSpline::Builder(samples_table).degree(3).build();
    getTestTheta(testThetas, bspline_sampler, chord_sum);

#define GETXY_TEMPLATE(DO_SOMETHING) [](DenseVector theta, double &x, double &y, const std::map<std::string, void *> &args) -> void { DO_SOMETHING }
#define VAR_TO_MAP(VAR) \
    {                   \
        (#VAR), &(VAR)  \
    }
#define MAP_TO_VAR(VAR, CLS) ((CLS *)args.find(#VAR)->second)

    int row = sqrt(len(Ns));
    int col = ceil((double)(len(Ns)) / row);
    for (int j = 0; j < len(Ns); j++)
    {
        auto N = Ns[j];
        DataTable samples_table_x, samples_table_y;
        for (int i = 0; i < N; i++)
        {
            DenseVector &&theta = getTheta(bspline_sampler, chord_sum * i / (N - 1));
            DenseVector X = sampler(theta);
            samples_table_x.addSample(theta, X(0));
            samples_table_y.addSample(theta, X(1));
        }
        //subplot(row, col, j + 1);
        for (int i = 0; i < len(degrees); i++)
        {
            auto degree = degrees[i];
            BSpline function_x = BSpline::Builder(samples_table_x).degree(degree).build();
            BSpline function_y = BSpline::Builder(samples_table_y).degree(degree).build();
            plotFunctionByTheta(testThetas, GETXY_TEMPLATE({
                x = MAP_TO_VAR(function_x, Function)->eval(theta);
                y = MAP_TO_VAR(function_y, Function)->eval(theta); }),
                                {{"label", "N=" + std::to_string(N) + ", degree=" + std::to_string(degree)}, {"linestyle", "dotted"}}, {VAR_TO_MAP(function_x), VAR_TO_MAP(function_y)});
        }
        plotFunctionByTheta(testThetas, GETXY_TEMPLATE({
            DenseVector X = (*MAP_TO_VAR(sampler, ISampler))(theta);
            x = X(0);
            y = X(1); }),
                            {{"label", "raw"}, {"linestyle", "dashed"}}, {VAR_TO_MAP(sampler)});
        show();
    }
#undef GETXY_TEMPLATE
    show();

    return 0;
}

double chordInterpolation(DataTable &samples_table, const ISampler &sampler, double start, double end, int N)
{
    double chord_sum = 0;
    DenseVector X0 = std::move(sampler(std::move(doubleToDenseVector(start)))), X00(2);
    X00 = X0;
    std::vector<DataPoint> points;
    double chord_len = 0;
    int direction = -1;

#define WORK(MAX_I, DO_SOMETHING)                                                                   \
    for (int i = 1; i < (MAX_I); i++)                                                               \
    {                                                                                               \
        DenseVector &&theta = doubleToDenseVector(start + direction * (end - start) * i / (N - 1)); \
        DenseVector X = sampler(theta);                                                             \
        chord_len += direction * sqrt(pow(X(0) - X0(0), 2) + pow(X(1) - X0(1), 2));                 \
        DenseVector &&chord = doubleToDenseVector(chord_len);                                       \
        X0 = X;                                                                                     \
        DO_SOMETHING                                                                                \
    }

    WORK((N / 4), { points.push_back(DataPoint(chord, theta(0))); })

    for (int i = points.size() - 1; i >= 0; i--)
        samples_table.addSample(points[i]);

    X0 = X00;
    chord_len = 0;
    direction = 1;

    WORK((N * 5 / 4),
         {
             if (i == N - 1)
                 chord_sum = chord_len;
             samples_table.addSample(chord, theta(0));
         })
#undef WORK

    return chord_sum;
}

size_t getTestTheta(std::vector<DenseVector> &testThetas, const Function &function, double chord_sum, size_t N)
{
    for (int i = 0; i < N; i++)
        testThetas.push_back(getTheta(function, chord_sum * i / (N - 1)));

    return testThetas.size();
}

void plotFunctionByTheta(const std::vector<DenseVector> &testThetas, GetXY getxy, const std::map<std::string, std::string> &keywords, const std::map<std::string, void *> &args)
{
    std::vector<double> x(testThetas.size()), y(testThetas.size());
    for (int i = 0; i < testThetas.size(); i++)
        getxy(testThetas[i], x[i], y[i], args);

    plotSpline(x, y, keywords);
}

DenseVector getTheta(const Function &function, double chord)
{
    DenseVector &&chordVector = doubleToDenseVector(chord);
    return doubleToDenseVector(function.eval(chordVector));
}